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ABSTRACT 



The Naval Postgraduate School (NPS) has been developing real-time 3D visual simula- 
tors on inexpensive commercially available graphics workstations. The effort to develop a 
3D visual simulation system, NPSNET, is an exploration and experimentation virtual 
worlds. Virtual world system have many goals including military training. This work is part 
of NPSNET. The current NPSNET system is kinematically based. The objectives of this 
work are motion dynamics and behavioral motion control for autonomous vehicles. Motion 
control is difficult when using dynamics due to the internal and external forces, however it 
enhances the realism as well as motion accuracy. We develop motion dynamics and behav- 
ioral control for ground-based vehicles. Motion dynamics and behavioral motion are an es- 
sential part of NPSNET for realistic battlefield simulation. 
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I. INTRODUCTION 



A. MOTIVATION FOR RESEARCH 

Recently simulation technology has been adapted in many ways. The flight simulator 
is used for training pilots and astronauts to safely control complex, expensive vehicles 
through simulated environments [Deyo, 89]. The capability of graphics workstations drops 
the relative cost of simulation, hence the technology has been moving gradually toward 
lower cost simulation environments through which the operator is also able to control his 
own viewpoint or motion. 

An advanced ground vehicle simulator is made up of a few, expensive subsystems that 
combine to create a complete virtual environment for the operator. Interactive real-time 
ground vehicle simulation demands scene realism and accuracy of motion. Because of the 
rapid development of computer technology, current computer hardware and software make 
it possible to synthesize realistic images. However, the real-time physical simulation of 
motion still remains a difficult task. It is now possible to use vehicle dynamics for 
simulating real world motion on a general purpose workstation. 

Dynamics can produce complex motion with minimal user inputs, but it is 
problematic in the difficulty of motion control and it is computationally expensive. A more 
automatic method of controlling motion can improve the control problem. Behavioral 
simulation is a means of automatic motion control according to certain rules. We can use 
behavioral control for autonomous moving vehicles in the simulation system. The research 
of motion dynamics and autonomous motion planning of a vehicle simulator on a general 
purpose workstation is a key component for the construction of a fully interactive virtual 
world. 
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B. NPSNET 



The Defense Advanced Research Projects Agency (DARPA) has been developing a 
simulation system entitled Simulation Networking (SIMNET) [Thorpe, 87]. SIMNET is an 
interactive battlefield simulation system designed for military training. SIMNET nodes 
require visual displays and specific hardware of a particular vehicle. The effort to develop 
a low-cost SIMNET type system based on commercially available graphic workstations has 
been an ongoing project at the Naval Postgraduate School (NPS). The system, NPSNET, is 
a real-time, 3D visual simulation system capable of displaying vehicle movement over the 
ground or in the air. NPSNET runs on Silicon Graphics workstations attached to a local area 
Ethernet and uses SIMNET databases and networking formats [Zyda, et al., 92], 

In NPSNET, up to 500 of the vehicles can be driving in the world at one time. The user 
can select any one of the active vehicles via mouse selection and control it with a six degree 
of freedom SpaceBall or button and dialbox. The pick button on the SpaceBall fires a round 
from the driven vehicle. Vehicles can be controlled by a prewritten script, driven 
interactively from other workstations, or autonomously via computer control. Displays 
show on-ground cultural features such as roads, buildings, and soil types. These vehicles 
move around in the virtual environment that is based on the terrain at Fort Hunter-Liggett, 
California. 

Depending upon the model of IRIS being used, the user can select to use texturing, 
and environmental effects such as fog and haze. A two dimensional (2D) map can be 
displayed that shows the position and tracking of all the players. This map displays the 
direction and viewing triangle of the driven vehicle as well as the position and movement 
of the remaining vehicles. The statistics and data concerning the driven vehicle (speed, 
pitch, roll, and so on) are displayed in an information window at the top of the screen. 
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C. OBJECTIVES OF THIS WORK 



The current NPSNET system is kinematically-based. The motion description, from 
user-input through actual frame rendering, consists of positions specified over time. The 
goal of this work is to add motion dynamics to NPSNET. The aerodynamics model is being 
developed concurrently for air vehicles [Cooke, 92], This work concentrates on the 
essential system issues for the implementation of virtual environments for real-time ground 
based vehicle simulation. 

1. Dynamics 

Dynamics deals with the motion of bodies under the action of forces. Motion 
occurs in the physical world as result of forces acting on objects. To achieve a degree of 
realism in motion control, the motion of objects must be simulated by the physical 
principles of dynamics. This work focuses on theoretical and numerical aspects of the 
implementation of a dynamic motion simulation system. 

2. Behavioral Control 

The search for more automatic methods of controlling motion is a major area in 
computer graphics. The problem of motion planning of autonomous vehicles consists of 
selecting the geometric path and vehicle speeds so as to avoid obstacles and it should 
minimize computationally expensive functions to keep the frame rate at an acceptable 
level. Motion is planned at a task-level and computed using physical laws. Inverse 
dynamics is the essence of the autonomous motion control. In higher level motion control, 
behavioral motion in particular, evaluates the state of the environment and generates 
motion accord ng to certain rules of behavior [Wilhelms, 90]. 

3. Vehicle Parameterization 

The vehicles used in NPSNET are military vehicles such as tanks, APCs and 
trucks. To get the realism and satisfy the simulation system users, the vehicles’ motion 
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should match the real vehicle specification. These specifications are defined in the data 
structure of the vehicles and incorporated with the dynamics equations. 

D. THESIS ORGANIZATION 

Chapter II covers basic dynamics in the vehicle simulation. Derived dynamics 
equations are defined and an integration method is presented. Chapter III provides vehicle 
modeling and motion control issues including dynamics as well as vehicle constraints. 
Chapter IV presents the high-level motion control issues, especially behavioral motion with 
inverse dynamics. Chapter V provides implementation details of the dynamic motion 
control and data structures into the simulator. Chapter VI covers conclusions and 
suggestions for further work in this area. 
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II. DYNAMICS 



A. INTRODUCTION 

This chapter presents the rigid body dynamics and coordinate system that are used in 
the simulation. A rigid body can be described as a fixed and unchanging extended mass. 
The dynamics of general plane motion of a rigid body combines translation and rotation. In 
the vehicle dynamics analysis, vehicles are treated as extended rigid bodies, and 
information concerning their mass, centers of mass, and mass distribution is needed. 

Previously, the dynamics equations of a vehicle model are derived [Meriam et.al., 86] 1 . 
Motion is produced by a simplified simulation of dynamics, that describes the linear and 
angular accelerations of a rigid body in terms of the forces acting on them. 

If a number of forces are acting on the body, their total translational effect can be 
found by merely summing them. The center of mass of the body will move translationally 
as if it were a particle mass influenced by one net force. The external forces acting on the 
vehicle consist of the frictional forces between the vehicle and ground, the normal forces, 
and the gravity force. A torque is similar to a force, except that it causes a rotational motion 
about a particular axis. Torques can be represented as 3D vectors describing their 
components about an x, y, and z-axis. 

B. DYNAMICS OF RIGID BODY MOTION 

The body coordinate system is used in the simulation. Since a rigid body is free to 
rotate and translate, it is possible to describe the placement of a rigid body in world space 
in terms of the location of the body’s center of mass and the body’s rotational orientation 
in world space. 



: The mathematical dynamics equations we cover below are taken from this reference and 
then implemented with vehicle constraints. 
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The basis vectors of this coordinate system are usually the principal axis of the object. 
For body i, the center of mass is represented by the vector c(t), and its orientation is 
represented by the 3x3 matrix R. The terms that are used in this chapter can be found in 
the Table 2.1. 



I 


Inertia Tensor Matrix 


f 


force 


fgrv 


Force due to gravity 


fext 


External applied force 


i 


Torque 


c 


Center of Mass 


r 


Position Vector 


R 


Orientation Matrix 


V 


Linear velocity 


a 


Linear acceleration 


P 


Linear momentum 


XJO 


Angular velocity 


w 


Angular acceleration 


l 


Angular momentum 


m 


mass 


At 


Time step between samples 


l x > h 


Moments of inertia 


l xy > l yz > ^xz 


Products of inertia 



Table 2.1. Dynamics Terms 



1. Center of Mass 

A representative particle of mass, m is located by its position vector, r,-, of the 

reference axis. The center of mass of the system of particles is located by the position 
vector, r, which, from the definition of the mass center as covered in statics, is given by 
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(Eq. 2.1) 



mr = Lm l r i 

where the total mass M = I m i [Meriam et. al., 86]. 

At time r, the center of mass is location in world space given by the weighted sum 



Im^CO _ Im/j (0 
I.m i M 



(Eq. 2.2) 



If using the body coordinate system, the center of mass in body coordinate is at the origin. 
This means that the body space is 



Em.MO 
M ° 



(Eq. 2.3) 



which means that Im,r 0i = 0 as well. 

In the motion simulation, the body coordinate system is used for the following 
relations. Let c(t) be the location of the center of mass in world space. Since the center of 
the mass is located at c(t), the location of the mass point in world space is 



r, (0 = R (t) r 0i + c (f) . At time r, since R «) = R(t) 6 = 6 , the Equation 2.2 is 



M 



Sm 1 (i?(0r oi + c(0) 

M 



Zm, 

= c(f )— i = c(t) 

M 



(Eq. 2.4) 



(r, (0 - c (0 ) = ^m 1 (i?(0r 0l + c(0 -c(t)) = i? (0 ^m,r 0l = 0 (Eq. 2.5) 
These relations enable us to consider linear translation separately from angular 
orientation. Thus it is possible to combine a rotation followed by a translation without 
changing the size or shape of the body. This can simplify the description of the laws of 
motion. 



2. Linear Momentum 

The linear momentum p of a panicle with mass m and velocity v is defined as 

p = mv (Eq. 2.6) 

Since the derivative of velocity is acceleration a, Newton’s law of motion for a panicle 
mass can be expressed as 
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(Eq. 2.7) 




This law says that the rate of change of the momentum p of a panicle is equal to the force 
/acting on the panicle. Since/ = p for a single particle, it is obvious to expect that a similar 
law holds for rigid bodies, comprised of collections of panicles. 

3. Angular Momentum 

There should be a linear relation between the body’s angular momentum and its 
rotational velocity. The angular momentum depends on the choice of origin of coordinates. 
The angular momentum Itft) of the panicle of the rigid body is 



Thus, /,( 1) is the cross product of the i ^ particle’s displacement from the center of mass and 
the momentum of the panicle with respect to the center of mass. The total angular 
momentum L(t) of the body is simply 



l t (t) = (r i (t)-c(t))xm i (r l (t) -c(t)) 



(Eq. 2.8) 



L(0 = £/,(*) = £(^(0 -c(t)) xm^r^t) -c(t)) 



(Eq. 2.9) 



y 




'A 



z 



Coordinate 



Momentum 



Figure 2.1 Cartesian Coordinate System And Momentum Of Particle 
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4. Inertia Tensor 



The inertia tensor of a body is a 3 x 3 matrix that describes the distribution of mass 
in the body. For symmetrical bodies, there are simple ways of calculating the moments of 
inertia. For a box centered at the origin with width c in x, b in y and a in z, multiplying the 
density by the volume gives the mass. In this case, the moment of inertia around the origin 
of x - axis is 

/,= ^m(a 2 + b 2 ) (Eq. 2.10) 



Similarly for y and z-axis, I y = -^-m (a 2 + c 2 ) and l z - (b 2 + c 2 ) . The off-diagonal 

1Z 1Z 

terms, such as are 



>0 



>0 



h y = J.yyV (x, y, z ) (xy) dxdydz = \\\\ J '\xydxdydz = 0 (Eq. 2. 1 1 ) 
2 2 2 T 2 ~ 

and similarly for the others because the integrals are all symmetric. Thus for the 

symmetrical block with width a in x, b in y and c in z, the inertia tensor matrix is 



; - " 

body ^2 



^2 , ^2 
0 + C 

o 
o 



0 

2 , 2 

a +c 



0 

0 



0 a 2 + b 2 



(Eq. 2.12) 



[Baraff, 91]. 



C. NEWTON-EULER EQUATIONS 



1. Linear and Angular Acceleration 

Two parts of Newton-Euler equations are the translational motion of its center of 
mass and rotational motion about the center of mass. The linear momentum and angular 
momentum are used in the simulation for the linear velocity and angular orientation each. 

By Newton’s law, the linear accelerations for x and z axis of the center of mass 
are derived from Equation 2.7. If c is the vehicle’s center of mass position and m is total 
mass, the equation of motion for linear acceleration a is second derivative of position c. 
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f x = mo r , f z = ma z 



(Eq. 2.13) 



where a is in meter/second . 

The torque, t, differs from a force in that the torque on a particle depends on the 
location of the particle. The torque acts on the particle, i, is \ = (r f (0 -c(t)) xF. By the 
Equation 2. 1 1 , the products of inertia are zero. So, the rotational equations for motion about 
the center of mass are, 

t x = I Z (6 X + (I z - I y ) io > co z (Eq. 2. 14) 

x y = 7 A + (/ * " “x®* (Eq. 2. 1 5) 

x z = “x 10 , (Eq. 2. 1 6) 

where the vectors are expressed in the principal axis frame of the vehicle. In the simulation, 

co is in radians/second and co in radians/second [Wilhelms, 87]. 

The dynamics equations are solved for new angular and linear accelerations. 
These accelerations must then be integrated to find new velocities and integrated again to 
find new positions. 

2. Integration 

The Euler method uses six equations shown in Equation 2.13, 2.14, 2.15, 2.16. 
Because of real-time limitations, methods that require only a single updated evaluation of 
/ for each time step are currently used. The Euler method is used for the integration. 

v t + &t = v t + afit (Eq. 2.17) 

e c + 8< = 6, + + 0.5co,Af 2 (Eq.2.18) 

c t + St = c t + v t At + 0.5a t kt 2 (Eq. 2.19) 

where 9 and c are the orientation and position at time t and t + At . 

The Euler method has two advantages. It is simple to program and efficient. The 
velocity is calculated once in a time step. Also it is efficient. This assumes acceleration is 
constant over the time period. The disadvantage of the method is the assumption that the 
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velocity is varied slowly in a time step. The inaccuracy problem, discontinuity between the 
current approximation speed and the next approximation speed, occurs when the time 
period is large or accelerations are changing rapidly. However with reasonably small time 
steps, the Euler method can produce appropriate result without too much trouble arising 
[Wilhelms, 87], 

In the simulation, the Euler method is used for this reason and the result is reasonable 
for ground based vehicles. The other method is investigated and implemented for air 
vehicles and both methods are compared in [Cooke, 92], 
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III. VEHICLE MODELING AND CONSTRAINTS 



A. VEHICLE MODELING 



In NPSNET, there are two types of ground vehicles: tracked and wheeled. All the 
vehicles can be approximated as rectangular blocks with the same mass density. The center 
of mass is assumed to be the center of the block. This assumption can reduce the dynamics 
calculation without the consideration of the vehicle types. The reference and path 
coordinates are shown at Figure 3.1. 

Body coordinates in the simulation for the vehicle description are subject to 

holonomic constraints' . The direction of the wheeled vehicle is computed by its front wheel 
angle, however the tracked vehicle’s direction is changed by the difference between the left 
and right track force. The motion of a vehicle is also limited by its constraints. In 
translational motion, the maximum acceleration is not always positive, nor is the maximum 
deceleration negative. This depends on the slope of the path. The engine forces of vehicles 
moving on bumpy terrain need to be carefully selected in order to maintain continuous 
contact with the ground as well as maintaining speed [Shiller et. al., 91]. 



r 




Path Coordinates 



Reference Frame 



Figure 3.1 Reference Frame And Path Coordinates 



~ Holonomic constraints mean <t> (c ; ) = 0 (constraints in precisely this functional form 
which can be used to eliminate dependent coordinates, c is body coordinates). Vehicles are 
assumed to be block structures. 
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1. Tracked Vehicle 




Tracked vehicles have two internal forces to move the vehicle, the left track force 
and the right track force. The two tracks of the tracked vehicle are assumed to be parallel. 
The distances from the center of mass to each track are the same. 

The internal forces,// and/ r , act on the vehicle at points other than the center of 

mass and cause vehicle rotation as well as translation. In Figure 3.2, if left and right forces 
are applied to the vehicle with the same direction and quantity, they produce translational 
motion without torque. However, two equal opposite forces cause a torque without a linear 
acceleration. The moments of inertia around the center of mass can be found from Equation 
2 . 12 . 

The translational factor, velocity, is computed by Equation 2.17 and the vehicle 
rotation about the center of mass is presented in Equation 2.15. From those equations, each 
axis’ velocities are 
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v x - v cos0, v z - usin0 



(Eq. 3.1) 



The x and z coordinates of new vehicle position can be obtained by Equation 2.19. 

Rotational velocity is merely calculated by the sum of two internal forces, 
f t = fi + f r . The opposite direction force has negative value. Torque is computed by this one 
force and angular orientation is obtained by Equation 2.18. 

2. Wheeled Vehicle 

The four-wheel vehicles with two fixed axles, such as the M-35 truck, are driven 
by fixed rear wheels and steered by the front wheels. Since the vehicle is simplified as a 
block structure, the motion of four wheels are neglected and the direction is computed as a 
parameter of the front wheels. The moment of inertia of the wheels is neglected and the 
wheels are making contact with the ground. 

The wheeled vehicle model is similar to the tracked vehicle in translational 
motion. But the steering angle and the vehicle orientation of the wheeled vehicle can be 

derived by non-holonomic constraints [Shiller et. ah, 91]. Choosing a center of mass as the 
guiding point, and assuming no sliding, makes the vehicle’s orientation non-holonomic 
since only incremental changes for vehicle orientation and steering angle are possible. 

The four tires are not treated as individual bodies, but those are considered for the 
orientation of the vehicle. In Figure 3.1, the r vector is normal to the surface, t is tangent to 
the path, and q is normal to both. A unit vector, n, in the rq plane is defined in the direction 
of the center of path curvature. At a velocity, v, the front wheel angle moves the vehicle 
along the path. In Figure 3.3, d is a distance from front axle to Center of Mass and the same 
for a distance to the rear, since we assume that all vehicles’ center of masses are located at 
the center of vehicles. 
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Non-holonomic constraints mean non-integrable relations involving the coordinates. 
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The direction of the wheeled vehicle is 



tana = . . . ■ . (Eq. 3.2) 

J p 2 -d 2 

where d is half the distance between the axles, and p is the distance to the Center of Turn 
from the mass center. The x and z-axis of the vehicle, that are parallel to the terrain, are 
determined based on the orientation and the steering angle at the previous point [Shiller et. 
al., 91]. 




Figure 3.3 Two Degree-Of-Freeedom Wheeled Vehicle Model 



The angle 6 is a function of the current vehicle position p and the desired direction 
of motion t. Since p is the result of the previous motions, it can be solved for the incremental 
changes in 0 [Shiller et. al., 91]. 

Figure 3.4 shows the vehicle at two positions: before and after an incremental 
move of As along the path from some point i. The mass center of vehicle at 2+1 is located 
at the end of the vector A st t from the mass center of the vehicle at i. During movement, the 
rear wheel moves along the p, direction to a distance a from the mass center of vehicle at i. 
The new location of mass center determines the orientation of the vehicle. Then can be 
defined as 
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Pj = cosGi; - sinG^p- 



(Eq. 3.3) 



The vehicle’s new orientation at i + 1 is 

P,.,- 3 P. + ^<, (Eq. 3.4) 

where steering angle is 

a = -Ascos0 i + Jd 2 - As 2 asin0 i (Eq. 3.5) 

Consequently, the angle 0 i + ] is 

0 i + 1 = acos(p i + 1 -t l+l ) (Eq. 3.6) 

The angle 0 j+ is used for the next computation as 0. 




Figure 3.4 Orientation Of Wheeled Vehicle By Two Positions 

B. VEHICLE CONSTRAINTS 

Several constraints between the vehicle and the environment are considered to ensure 
vehicle dynamic stability along the path. Various forces such as friction, gravity, engine 
and brake torque are included in the simulation. 

The external forces acting on the vehicle consist of the friction forces between the 
vehicle and ground, the normal forces and the gravity force. The total external forces are 
the sum of those forces. 
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1. Engine And Brake Constraints 

Engine torque produces frictional force and pushes the vehicle either forward or 
backward. A positive engine torque produces a force in the direction of motion, while a 
negative torque, or a braking force, produces a force in the opposite direction. This force is 
bounded by the maximum equivalence engine force F ^ and maximum braking force F mn . 

F min <f t <F max (Eq. 3.7) 

The force f t is the internal force of the vehicle. The limitation of the forces is 
validated by the real vehicle specification. The rpm value of the engine is changed by 
internal force not the vehicle speed. The engine torque constraint limits only the 
acceleration. 

2. Friction 

Friction is a complicated phenomenon and modeling it exactly is challenging. An 
approximate treatment is good enough for most engineering purposes, and suffices to 
produce visually reasonable effects on motion [Wilhelms, 88]. 

The tracks or wheels are making contact with the ground. At each contact point, 
there are three unknown forces: two friction and one normal force. Friction forces act to 
oppose tangential motion along the surface and are dependent upon the normal force 
pressing into the ground. 

Basically, each surface is assigned a stick angle. The intuitive meaning of this 
angle is that an object will sit motionless on that surface as long as the tilt relative to gravity 
is less than the stick angle. As soon as the tilt exceeds that limit, the vehicle starts to slide. 

The total friction force, Ft ric , tangent to the xz plane, is represented in path 
coordinates as 
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F fric = U+fqQ 



(Eq. 3.8) 



where f t and f are the components tangent and normal to the path, respectively. The 

equation of motion of the vehicle can be written in terms of the tangential speed v and the 
tangential acceleration a . 

2 2 n 

f t t + f q q - mgk - mknv +mta (Eq. 3.9) 

where k is path curvature, g is gravitational acceleration and m is the vehicle mass. The 
reaction force in the r direction when the vehicle moves along the bumpy terrain is 
neglected. The projection of the external forces in the t, q, and r directions is obtained by 
dot multiplying both sides of Equation 3.6 with the vector t, q and r. 

f t - mgk, + ma (Eq. 3.10) 

f q = mgk q + mkn q v 2 (Eq. 3.11) 

In world coordinates, the friction forces in thexz coordinates are 

F fnc-x = ft cos e-/, sine (Eq. 3.12) 

F fnc - z = ft Sin 9 + fq C0S 0 ( Et l- 3-13) 

The friction force required by vehicle motion is J F } rtc - x + rfric-z' that is divided 
into dynamic friction and static friction. Dynamic friction is applied to moving vehicles and 
static friction is applied to unmoving vehicles in the simulation. 

3. Gravity 

The effect of gravity is easily calculated given the gravitational acceleration 

(about 9.81 m/sec on the earth’s surface). For the center of mass on an inclined surface, 
the applied gravity force is 
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f gro = (0, -9.81,0) m 



(Eq. 3.14) 



If the inclination angle is 0, then h = (-sine,cos6,0) . That is the gravity force that applied 
to the vehicle is 



If the gravity force F grav exceeds the friction force Ff ric of the unmoving vehicle on an 
inclined surface, the vehicle should slide either forward or backward. 

The torque due to this force acting in the body fixed coordinate frame is 



while c is the center of mass and t is angular torque. This torque acts on the pitching and 
rolling of the vehicle. The reaction force by this torque is neglected in the simulation. 
Maximum pitching and rolling angles are determined by the slope of the terrain. Hence 
during the simulation, vehicles continuously contact the ground. In Figure 3.5, e p is 
pitching angle and 0 r is rolling angle. 



F gru = -» ' fgru = mg cos9 



(Eq. 3.15) 




(Eq. 3.16) 



-mg 



-mg 




Figure 3.5 Gravity And Mass On An Inclined Surface 
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IV. BEHAVIORAL CONTROL 



A. ADAPTIVE MOTION CONTROL 

The problem of motion planning of autonomous vehicles consists of selecting the 
geometric path and keeping the appropriate vehicle speed so as to avoid obstacles. Adaptive 
motion control means that the environment has an impact on the vehicle motion and vice 
versa. Information about the environment and the vehicle such as location and orientation 
of objects and vehicles must be available during the control process. Adaptive motion 
makes possible goal-directed and constrained behavior, since it allows the user to describe 
movement in terms of relations among objects and vehicles. The user only specifies the 
broad outline of the motion, then the simulation system fills in the details [Zeltzer et. al., 
89]. 

Behavioral control is high-level control, however the motion is achieved by 
dynamics. It evaluates the state of the environment and generates motion according to 
certain rules of behavior [Wilhelms, 90]. In the simulation, obstacle avoidance is a basic 
rule of behavioral motion. Obstacle avoidance is a simple model of adaptive motion control 
and it is implemented for the autonomous vehicles in the simulation. The goals for 
behavioral control are to develop effective techniques for planning and following routes 
based on object information. 

In NPSNET, the object locations and terrain elevations are maintained in a text format 
file. Sensing detects the characteristics of obstacles in the environment and passes this 
information to the vehicles and the motion control routine. Sensors have a fixed position 
and orientation on the body. To save memory and improve the frame rate during the 
execution, the scope of obstacle detection is limited. All fixed objects are attached to the 
terrain, thus only the geometric data and object data in the field of view are used in obstacle 
detection. 
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B. IMPACT OF ENVIRONMENT AND VEHICLE MOTION 



1. Searching Range And Detection 

Obstacle avoidance motion is defined in two dimensions since all objects are 
ground-based. The information available on objects is their center coordinates and radius. 
We develop techniques necessary to detect obstacles with minimum delay between sensing 
and acting. During the execution, the information concerning the objects is maintained as 
an array. Only the objects in the field of view are kept in the array. There are more than 
40,000 natural and man-made objects in the Fort Hunter-Liggett terrain database used in 
NPSNET. Distance between an object and a vehicle can be obtained with a two 
dimensional computation. 

The field of view, as the angle in radians, about the present object heading is more than 
enough. The searching angle is narrow than the field of view. The searching angle 9 r 9 r is 

10 degrees. This angle gives the vehicle with speed 60 km/h approximately 12.9 meters 
path corridor on the terrain. 



Left 

Search Trii 




Right 

Search Triangle 



Figure 4.1 Searching Area 
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All objects are not displayed at one time. The objects which are located in the 
current field of view, are shown and those are kept in the avoidance object array. The range 
distance d, is determined by the speed of the vehicle. 

d ~ d minimum + ^speed (Et}4.1) 

where d m i m i num is 20 meters and d spee j is the vehicle speed in meters/second. 

Our search heuristic is left triangle first. Only one object can be detected at one 
time. The vehicle can change its path to avoid an object and then the vehicle starts the next 
search. 
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Determine JlangeJDistance (); 

Left_search_Angle = Vehicle ^Direction - HFOS; 

Right JSearchj\ngle = Vehicle _Direction + HFOS ; 

/* Sensing coordinates in x,z coordinates *f 
Sensing jyosition = Current _Ve hide ^Coordinates ; 

/* Determine left and right search area *1 
Determine JBoundaryJ_.eft_Search_Area ( ); 

Determine ^Boundary _Right_Se arch _Area (); 

l* Looking for any obstacles in this area */ 

Left_Search (); 

Right _Search (); 

if(LEFT && RIGHT) I* Detect obstacles in left and right area */ 

/* If the obstacles are the same one */ 
if (LeftjDbject == Right jObject) 

DETECTION = LEFT ; 
else /* If the obstacles are different one */ 

/* Measure the distance between the vehicle to the obstacles */ 
Left_Distance ~ Distance _In_2D (Sensing _Position, LeftjDbject); 
Right _Distance = Distance Jn_2D (Sensing _Position t Right JObject); 

if (Left_Distance >= Right JDistance) 

Detection = RIGHT; 

else /* Left obstacle is nearer than right one */ 

Detection = LEFT; 

else 

if (Left) 

Detection = LEFT; /* Obstacle in left area */ 
else if (Right) 

Detection = RIGHT; I* Obstacle in right area *f 

else 

Detection = FALSE; /* M? avoidable obstacle */ 



Figure 4.2 Algorithm For Obstacle Detection 
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2. Inverse Dynamics 

The inverse dynamics problem of a moving vehicle consists of solving for vehicle 
orientation and translation. Translation and orientation of the vehicle can be derived from 
dynamics equations presented in Chapters II and III. 

The translational inverse dynamics problem is defined as an autocruise control in 
the simulation. The vehicle velocity is fixed when autocruise mode is selected. To get an 
appropriate engine force, we need the information of variance of external forces. The 
engine force must change due to the change of external forces. The summation of new 
internal forces of the vehicle and total external forces remains constant through the 
computation. Autocruise speed is computed by this constant force, hence the vehicle 
velocity is kept fixed. Autocruise speed control helps interactive user control during driving 
too. 

The direction of the wheeled vehicle is presented in Chapter III. Referring to 
Figure 3.4, the new locations of the mass center and the rear axle define the new vehicle 
orientation [Shiller et. al., 91], From Equation 3.4, it is possible to solve the steering angle 
a that moves the mass center along path in terms of current vehicle’s direction 0 . The next 
steering angle a is obtained from the Equation 3.2 and Figure 3.3. 

a= atan(2tan0) (Eq. 4.2) 

For the tracked vehicle, total engine force is derived from the autocruise control 
routine. Then the left and right forces of the vehicle are changed until the vehicle’s new 
orientation matches the desired orientation. Figure 4.3 shows the algorithm for inverse 
dynamics of the vehicle direction problem. 
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/* For the tracked vehicle */ 
if (Detect jObject) { 

Compute_Desired_Orientation( ); 
do while (NewjOrientation /= DesiredjOrientation) { 
if (Object _I nJLeft Sear ch_Tri) 

Left _Track_Force = Left_Jrack_Force - Delta; 
Right_Track_Force = Right _Track_F or ce + Delta; 
else I* Object in right search tri *1 

Left _Track _Force = Left_Track_Force+ Delta; 
Right_Track_Force = Right JTrack_F or ce- Delta; 
Compute _N ew _Orientation( ); 

} 

} 

/* For the wheeled vehicle */ 
if (Detect -Object) { 

C ompute _Desired jOrientation(); 

do while (New -Orientation /= DesiredjOrientation) { 

Compute_New_Orientation(); /* Equation 4.1 */ 

; 

} 



Figure 4.3 Algorithm For Orientation And Desired Path 
3. Path Determination 

The desired path of the vehicle is simple. The behaviors are grouped into two 
activities for the set of path determination. The first activity is to travel when the vehicle is 
in a clear area, and the second is to control the vehicle when obstacles are present. When 
the vehicle detects an object in the left search triangle, T/, it changes its direction to the 
right. The other case, when it detects any object in the right search triangle, it changes its 
direction to the left. In the case when obstacles are presented in both the left and right 
triangles, the vehicle avoids the nearest obstacle. The distance is computed two- 
dimensionally. The new orientation of the vehicle is 

D desired ~ ^current ± 0.2571 (Eq. 4.3) 

where all the values are radians. 
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Vehicle 



Figure 4.4 Obstacle Avoidance Path 

C. FLOW OF BEHAVIORAL MOTION 

The behavioral motion control routine is shown in Figure 4.5. First, the vehicle 
determines the searching distance and checks the object list whether any objects are in the 
search area or not. When the vehicle detects an object in the left search triangle, it changes 
its direction to the right. When it detects any objects in right search triangle, it changes its 
path to the left to avoid the object. The desired direction is matched to the current vehicle 
orientation, then the vehicle resumes searching again. 

Motion is produced by the simplified simulation of dynamics presented in Chapter II 
and III. Object avoidance control must avoid the nearest imminent obstacle. This method 
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cause the vehicle to turn and collide with the neighbor obstacle, since we do not make path 
planning for behavioral motion. 




Figure 4.5 Behavioral Motion Control Flow 
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V. IMPLEMENTATION ISSUES 



A. SYSTEM OVERVIEW 

Dynamic simulation has been implemented on Silicon Graphics IRIS graphics 
workstations. The units of the metric system that we use in this work are found in Table 5.1. 
For that reason, we convert those terms in the simulation then display in the Information 
Window as user friendly terms such as degrees and km/h. 

When the system is initialized, the main window displays the field of view from the 
vehicle as a default. Vehicle parameters and information are shown in the upper window as 
text. The flow of control of the system is illustrated in Figure 5.1. 

The approach to implementing motion dynamics is pre-processing control. In pre- 
processing control, two sequential steps are involved. First, appropriate forces and torques 
are found. All forces and torques are summed to produce a net force and torque acting on 
a vehicle, and these are used in the second step, dynamic analysis. The advantage of pre- 
processing is the dynamics control routines can be added and removed easily. 





Display Unit 


Computation Unit 


Acceleration 


kg m /sec 


kg m /sec 


Speed 


Kilometers / hour 


meters /second 


Angle 


Degree 


Radian 


Mass 


tons 


kg 


Length 


meter 


meter 



Table 5.1 Parameter Unit Table 
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Figure 5.1 System Flow Overview 

B. VEHICLE PARAMETERS 



The 3D vehicle objects use text based NPSOFF file format [Zyda, 91]. Since NPSOFF 
is an application independent description of graphical objects, objects can be designed and 
maintained by general purpose tools [Zyda et.al., 92]. To explore the motion dynamics and 
behavioral simulation, the vehicle object data structure contains both graphics rendering 
and motion control variables shown as the following. We use structure type for vehicle 
parameters shown at Figure 5.2. Figure 5.2 shows variables and their descriptions. 
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float 


limit_speed 


maximum vehicle speed 




limit_rpm 


maximum engine rpm rate 




max_power 


maximum engine power 




manpower 


maximum brake power 


float 


pos[3J 


position coordinates in 3-D 




eye[3] 


view position coodinates 




lookatpt[3] 


viewing coordinates 




lookfmpt[3] 


viewing position coordinates 




direction 


vehicle direction 




viewdirection 


viewing direction 




wheel_angle 


front wheel angle for wheeled vehicle 




elev 


elevation data 




gas 


remained gas amount 




rpm 


throttle value 




length 


vehicle length 




height 


vehicle height 




width 


vehicle width 




cruise_speed 


cruise speed when the cruise control is on 


float 


vel[3] 


velocity value x, y, z 




scalvel 


speed of the vehicle 




acc[3j 


acceleration value x, y, z 




scalacc 


acceleration of the vehicle 




force 


total external force to the vehicle 




1 force 


left track force 




rforce 


right track force 




surge_force 


total engine force of the vehicle 




drag_force 


total drag external forces to the vehicle 




g_force 


external force by the gravity 




net_torque 


net torque to the vehicle 




torque 


total torque to the vehicle 




roll 


rolling moment 




pitch 


pitching moment 




mass 


net weight of the vehicle 



Figure 5.2 Ground Vehicle Structure 
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C. VEHICLE MOTION CONTROL 



1. Integration Of Forces 

The external forces and internal forces are integrated to find new velocity and 
orientation. All considered forces are defined in Chapter III. Depends on the user selection, 
the integrated forces are applied to the vehicle’s velocity and direction. All applied forces 
are defined in Chapter III. Figure 5.3 shows the algorithm of the summation of total forces. 
These forces result the accelerations using first-order equation f(t, p) described in Chapter 
II. Because of real-time limitations, we use only a single updated evaluation of/ in each 
time step. 

2. Vehicle Speed And Direction 

The vehicle speed is computed by linear acceleration using Newton-Euler 
equations. Newton-Euler equation is a first order equation in the translational and angular 
velocities. To compute the vehicle position and direction, we use improved Newton-Euler 
equations defined in Chapter II. In kinematically based NPSNET, users can change the 
vehicle speed using SpaceBall or keyboard directly. And the vehicle speed is kept as a 
constant unless users do not change the vehicle speed. However in dynamic motion control, 
the vehicle speed is continuously changed by external forces. This makes the user use 
dynamic motion control difficult. Figure 5.4 shows the algorithm for vehicle’s acceleration 
and velocity. The maximum velocity is limited by the vehicle specification independently 
from the variance of forces. 

Figure 5.5 shows the algorithm for the computation of vehicle direction. As we 
mentioned in Chapter III, the vehicle’s direction is achieved by two methods, depending on 
vehicle type. Users can drive the vehicle using either the keyboard or SpaceBall. 

The integration of all forces and vehicle speed is shown in Figure 5.6. The new 
vehicle’s linear and angular velocity determine the next position and direction of the 
vehicle. This parameters render the next frame of the simulation. 
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integrationf) 

{ 

/* compute inertia tensor */ 

calc_inertia(); /* compute inertia tensor of the vehicle *1 
angle jnomentum(); I* compute maximum roll and pitch */ 
sumjinear Jorces(); /* compute surge force and drag force for the vehicle */ 
sum_angular Jorces(); /* compute vehicle orientation forces *1 

if(lbehavior) /* user interactive control */ 

{ 

if(!cruise Jlag) /* when cruise mode off *1 

{ 

/* compute linear and angular velocities */ 

calc_acceleration(); 

calc_velocity(); 

calc_iorque(); 

calc_direction( f ): 

} 

else /* when cruise mode on */ 

{ 

cruise _on(); /* cruise speed computation */ 
velocity = cruise_speed; 
calc jorque(); 
calc_direction(); 

} 

J 

else /* behavioral control *1 

{ 

/* compute all the internal and external forces */ 
cruise _on(); 

velocity = cruise _speed; 
behavior _motion( ); 
calcJnversejdirection() ; 
calcjorqueO; 
calcjdireciionO ; 

} 



Figure 5.3 Algorithm For Integration Of All Forces 
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/* compute the linear acceleration of the vehicle */ 
calc_acceleration( ) 

{ 

/* compute acceleration by the Newton s second law F - ma */ 
acceleration = force I mass; 

/* divide the acceleration to x axis and z axis */ 
acceleration^ ] = acceleration * fcos(vehiclejdirection); 
acceleration[Z] = acceleration * fsin(vehicle_direction); 

} 

/* compute the linear velocity of the vehicle *1 
calc_velocity() 

{ 

I* compute the speed from the acceleration in m/sec unit */ 
velocity += deltatime * acceleration; 

/* limit the max speed between minimum and the maximum vehicle speed *1 
iffvelocity <= MINSPEED) velocity = MINSPEED; 
iff velocity >= MAXSPEED) velocity = MAXSPEED; 

velocity[X] = velocity * fcos(vehicle_direction); 
velocity [Z] = velocity * fsinfvehicle ^direction); 

j 



Figure 5.4 Algorithm For Vehicle Acceleration And Velocity 
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/* compute angular acceleration and angular velocity by Euler method w = w * dt */ 
calc_direction() 

{ 

if '(vehicle type == TRACKED ) 

{ 

/* omega - torque / inertia tensor */ 
rotation = (netjorque / inertia[Y]); 
dt_rot = rotation * deltatime; 
direction + = dt_rot; 

} 

else /* Wheeled vehicle */ 

{ 

/* compute the difference between the wheel angle and direction *1 
tangle = f tan( wheel _angle); 
rotation = fatan(0.5 * tangle ); 

/* if there are difference between the left and right forces 
it should be added to the vehicle direction value */ 
iff left Jorce != right Jorce ) 

{ 

delta _rotation = (netjorque / inertia[Y]); 
rotation += delta jotation; 

} 

/* wheeled vehicle cannot rotate without translation */ 
iftvelocit) != 0.0) 

{ 

di_rot = rotation * deltatime ; 
direction += dt_rot; 

} 

} 



Figure 5.5 Algorithm For Vehicle Direction Computation 
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Figure 5.6 Motion Control Flow 
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D. BEHAVIORAL CONTROL 



An autonomous vehicle requires perception, planning and control to act intelligently. 
Behavioral control is designed to explore use of dynamics both for interactive simulation 
and automatically controlled simulation. The goal of behavioral control in this simulation 
is to build a vehicle that can drive autonomously in the cultural and non-cultural objects 
environment. The behavior rule is avoidance of all obstacles. 

The method of obstacle avoidance in this simulation is simple. Figure 5.7 shows the 
process for obstacle avoidance. A vehicle’s behavioral motion is assembled in sensing, 
planning and control. Function details are attached in Appendix B. The flow of behavioral 
motion is 

Sensing: Obstacle detection, Figure 4.2 

Planning: Path determination, Figure 4.3 

Control: Inverse dynamics computation for vehicle movement 



behavior _motion( ) 

{ 

search _distance = fabs(velocity * 3.6); 

detect _bounds (); /* determine ser aching boundary */ 

search_object( ); /* obstacle detection */ 

if(detect) /* path determination */ 

{ 

if(whichside == LEFT) 

dir _value += FINE -PI; /* turn the vehicle right *1 
else I* the object is on the right side */ 

dir_value -= FINE_PI; /* turn the vehicle left */ 

} 

else /* no avoidable objects */ 
dir _value = 0.0; 

inverse_dynamics(); /* compute inverse dynamics for vehicle motion */ 

} 



Figure 5.7 Algorithm For Obstacle Avoidance 
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E. USER INTERFACES AND PERFORMANCE 



User interfaces have been developed to show the current status and vehicle 
information. The upper window in NPSNET displays current status of the system. The 
Main Window displays the view from the vehicle as a default. 

The vehicle dynamics model needs to know what the user is doing. Data acquisition 
is performed on the steering, brake, and other controls that are available to maneuver the 
vehicle. User input devices are shown in Appendix A. 

This simulation is done in real-time. The performance of NPSNET is not affected by 
dynamics. Performance of the current simulation system is approximately ten frames per 
second without texturing, and six frames per second with texturing. We find the effects of 
dynamic motion control are more realistic than kinematics-based control on bumpy terrain. 
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IV. CONCLUSIONS AND FURTHER WORK 



Current graphics workstations support visual realism and high-performance 
computing. In this work, we investigate motion accuracy based on the physical laws to 
improve the NPSNET vehicle simulation. We have used the dynamics of rigid bodies and 
Euler equations for solving their equations of motion and a behavioral motion based on 
these dynamics. 

The simulation methods presented in this paper are implemented and run in real-time. 
Constraint dynamics results in reasonable vehicle motion as we expected. The application 
of dynamics for high-level motion control is working correctly and we can certify dynamics 
for low-level motion control in the next extension of the simulation. 

At present, we assume that vehicles are simple block structures to decrease the 
computational complexity and the Euler equation is selected for the same reason. We have 
not thoroughly explored all the possibilities for supporting the vehicle model. We do not 
consider complex natural parameters for this computation such as surface characteristics of 
the ground, tip-over forces and rebounding forces. For avoidnace motion, since vehicles do 
not save their past paths, current behavioral motion is not quite intelligent to select optimal 
path. 

This work suggests a number of topics for further research. Dynamics can be used for 
the low-level motion control and this can provide for many high-level control issues. The 
vehicle simulation should handle multi-body vehicles. The higher performance of the 
graphics workstations and dynamics can lead the vehicle simulation realistic. As hardware 
performance improves, the vehicle dynamics can take advantage of the faster throughput. 
And more intelligent behavioral motion is a future avenue of work. 
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APPENDIX A 



USER INPUT DEVICES LAYOUT 




Button Box 

□ □ □ □ 

Drive 0 12 3 

□ □ □ □ □ 



Drive 4 5 6 7 8 9 

□ □□□□□ 

Drive 10 11 12 13 14 15 

□ □□□□□ 

Drive 16 17 18 19 20 21 

□ □□□□□ 

Drive 22 23 24 25 26 27 

Drive POOP 
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Space Ball Buttons 

















stop 


zero 


zero 




zero 


View 


Gun Elev View Ti 

















nt 



Sense Sense From/To SpaceBall 



Pick: Fire 




S 




F 




M 


E 




1 




E 


L 




R 




N 


E 




E 




U 


C 










T 











Mouse 
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APPENDIX B 



Source Code For Dynamic Motion Control 



/* 

* Dynamic motion control by the physics laws 

* Use Eulerian method for the velocities and positions 
*/ 

#include “vehsim.h” 

#include “vehcle.h” 

/* External dynamics forces variables in static */ 

float friction; /* Friction between the vehicle and the terrain */ 

float air_resist; /* Air resistance to the vehicle */ 

/* compute the dynamics of the vehicle the tilt and roll */ 

angle_momentum(vehobj) 

struct vehicle *vehobj; 

( 

float fp[3],sp[3]; 

float xpos,ypos,zpos; /* x,y,z position of center of mass */ 

/* The pitch and roll are computed by taking a point 2 meters 
in front to get the pitch and 2 meters to the right sideto get the roll */ 

frontpoint[X] = fp[X] = vehobj->pos[X] + fcos(vehobj->direction) * 2.0; 
frontpoint[Z] = fp[Z] = vehobj->pos[Z] + fsin(vehobj->direction) * 2.0; 
frontpoint[Y] = fp[Y] = gnd_level(fp[X],fp[Z]); 

sidepoint[X] = sp[X] = vehobj->pos[X] + fcos(vehobj->direction+HALFPI) * 2.0; 
sidepoint[Z] = sp[Z] = vehobj->pos[Z] + fsin(vehobj->direction+HALFPI) * 2.0; 
sidepoint[Y] = sp[Y] = gnd_level(spfX],sptZ]); 

/* get the Y position of the vehicle */ 
xpos = vehobj->pos[X]; 
zpos = vehobj->pos[Z]; 
ypos = gnd_level(xpos,zpos); 
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i 



/* positive pitch is when the nose goes up */ 
vehobj->pitch = fatan((fp[Y] - ypos)/2.0) / DEGTORAD; 
if (vehobj->pitch < 0.0) vehobj->pitch += 360.0; 

/* positive roll is when the right side goes down */ 
vehobj->roll = fatan((ypos - sp[ Y])/2.0) / DEGTORAD; 
if (vehobj->roll < 0.0) vehobj->roU += 360.0; 



/* 

/* compute gravity force on an inclined surface 
*/ 

calc_gforce(vehobj) 
struct vehicle *vehobj; 



float pitch_angle; 
float rad_angle; 

pitch_angle = vehobj->pitch; 

/* compute the sliding value from the pitching angle of the vehicle */ 
if(pitch_angle ==0.0) { 
rad_angle = 0.0; 

} 

/* sliding force is the SIN value of the grade */ 
else 
{ 

if(pitch_angle > 0.0 && pitch_angle < 90.0) /* the vehicle nose up */ 
rad_angle = fsin(pitch_angle * DEGTORAD); 

if(pitch_angle < 360.0 && pitch_angle > 270.0) /* the vehicle nose down*/ 
rad_angle = fsin((360.0 - pitch_angle) * DEGTORAD); 

} 

/* f = mg x sinO in case of g=9.81 and ground elevation */ 
if(vehobj->scalvel != 0.0) 

( 

if(vehobj->scalvel > 0.0) /* if the vehicle moves forward */ 

{ 
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if(pitch_angle > 0.0 && pitch_angle < 90.0) 

vehobj->gJorce = (vehobj->mass * GRAVITY * rad_angle); 

else 

vehobj->g_force = -(vehobj->mass * GRAVITY * rad_angle); 

) 

else /* the vehicle moves backward */ 

{ 

if(pitch_angle > 0.0 && pitch_angle < 90.0) 

vehobj->g_force = -(vehobj->mass * GRAVITY * rad_angle); 

else 

vehobj->g_force = (vehobj->mass * GRAVITY * rad_angle); 



else /* the vehicle speed = 0 */ 

{ 

if(vehobj->pitch > 0.0 && vehobj->pitch < 90.0) 

vehobj->g_force = -(vehobj->mass * GRAVITY * rad_angle); 
else 

vehobj->g_force = (vehobj->mass * GRAVITY * rad_angle); 



} /* end g_force */ 



/* 

/* compute the drag forces to the vehicle, friction specifically 

V 

calc_drag_force(vehobj) 
struct vehicle *vehobj; 

( 

float vel; 

/* compute the air resistance to the vehicle */ 

/* approximate the value to the function of square of velocity */ 
if(vehobj->scalvel > 0.0) 

air_resist = vehobj->scalvel * vehobj->scalvel * 10; 

else 



if(vehobj->scalvel != 0.0) 

air_resist = -(vehobj-> seal vel * vehobj->scalvel * 10); 
else /* the speed is 0 */ 
air_resist = 0.0; 
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/* compute the friction between the ground and the vehicle */ 

/* approximation coefficient between the velocity and friction */ 
vel = fabs(vehobj->scalvel); 

if(!SIDE_BREAK) 

{ 

if(vehobj->scalvel == 0.0 && vehobj->scalacc = 0.0) 

/* The vehicle is stalled */ 
friction = 0.0; 

else if(vehobj->scalvel == 0.0 && vehobj->scalacc != 0.0) 
/* static friction */ 

friction = SFRICTION + 1000.0 + (vehobj->mass); 

else if(vehobj->scalvel > 0.0 && vehobj->scalacc != 0.0) 

/* dynamic friction */ 

friction = DFRICTION + (vehobj->mass); 



else if(vehobj->scalvel < 0.0 && vehobj->scalacc != 0.0) 
friction = -(DFRICTION + vehobj->mass); 



vehobj->drag_force = -(friction + air_resist); 
} /* emd drag_dorce */ 



/* 

/* compute the engine force to the vehicle and breaking force 

V 

calc_engine_force(vehobj) 
struct vehicle *vehobj; 

{ 

if(Stop_Engine == FALSE) 

{ 

/* compute the linear translation surge force by the summation of the left and right forces */ 
vehobj->surge_force = vehobj->rforce + vehobj->lforce; 
if(vehobj->scalvel > 0.0) 

{ 

if(vehobj->surge_force >= MAXPOWER) 

vehobj->surge_force = MAXPOWER; 
if(vehobj->surge_force <= MINBREAK) 

vehobj->surge_force = MINBREAK; 
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} 

if(vehobj->scalvel < 0.0) 

{ 

if(vehobj->surge_force <= M INPOWER) vehobj->surge_force = MINPOWER; 
if(vehobj->surge_force >= MAXBREAK) vehobj->surge_force = MAXBREAK; 

} 



} 

else 

{ 

vehobj->surge_force = 0.0; 
vehobj->torque = 0.0; 

} 

) /* end engine force */ 

/* 

/* compute inertia tensor for each axis 
*/ 

calc_inertia(vehobj,inertia,inertia_matrix) 
struct vehicle *vehobj; 
float inertia[6]; 
float inertia_matrix[3][3]; 

{ 

/* assume that the vehicle is block wih the same mass density */ 

/* principal axes inertia */ 

inertia[X] = (vehobj->mass * (vehobj->length * vehobj->height)) / 12.0; 
inertia[Y] = (vehobj->mass * (vehobj->width * vehobj->length)) / 12.0; 
inertia[Z] = (vehobj->mass * (vehobj->width * vehobj->height)) / 12.0; 

/* secondary axes inertia in block structure */ 

inertia[XY] = (vehobj->mass * (vehobj-> width * vehobj->height)) / 2.0; 
inertia[XZ] = (vehobj->mass * (vehobj-> width * vehobj-> length)) / 2.0; 
inertia[YZ] = (vehobj->mass * (vehobj->length * vehobj->height)) / 2.0; 

* make an inertia tensor matrix */ 
inertia_matrix[X][X] = inertia[X]; 
inertia_matrix[X][Y] = -inertia[XY]; 
inertia_matrix[X][Z] = -inertia[XZ]; 
inertia_matrix[Y][X] = - inertia[XY]; 
inertia_matrix[Y][Y] = inertia[Y]; 
inertia_matrix[Y][Z] = inertia[YZ]; 
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mertia_matrix[Z][X] = -inertia[XZ]; 
inertia_matrix[Z][Y] = -inertia[YZ]; 
inertia_matrix[Z][Z] = inertia[Z]; 

} /* end calc Jnertia */ 

/* 

* b is the determinant of 3x3 matrix a 
*/ 

det(a,b) 
float a[3][3]; 
float *b; 

{ 

*b = a[X][X] * (a[Y][YJ * a[Z][Z] - a[Y][Z] * a[Z][Y]) 

+ a[X][Y] * (a[Y][Z] * a[Z][X] - a[Y][X] * a[Z][Z]) 
+ a[X][Z] * (a[ Y] [X] - a[Z][Y] - a[Y][Y] * a[Z][X]); 



/* 

* compute inverse matrixof 3X3 inertia matrix 
*/ 

calc_inverse_inertia(inertia_matrix,inv_inertia_matrix) 
float inertia_matrix[3][3]; 
float invjnertia_matrix[3][3]; 

{ 

float d; 

det(inertia_matrix,&d); 

inv_inertia_matrix[X][X] = (inertia_matrix[l][l] * inertia_matrix[2][2] 

- inertia_matrix[l][2] * inertia_matrix[2][l])/d; 
inv_inertia_matrix[Y][X] = (inertia_matrix[Y][Z] * inertia_matrix[Z][X] 

- incrtia_matrix[Y][X] * inertia_matrix[Z][Z])/d; 
inv_inertia_matrix[Z][X] = (inertia_matrixtY][X] * inertia_malrix[Z][Y] 

- inertia_matrix[Y][Y] * inertia_matrix[Z][X])/d; 
inv_inertia_matrix[X][Y] = (inertia_matrix[Z][Y] * inertia_matrix[X][Z] 

- inerlia_matrix[Z][Z] * inertia_matrix[X|[Y])/d; 
inv_inertia_matrix[Y][Y] = (inertia_matrix[Z][Z] * inertia_matrix[X][X] 

- inertia_matrix[Z][X] * inertia_matrixtX][Z])/d; 
inv_inertia_matrix[Z][Y] = (inertia_matrix[Z][X] * inertia_matrix[X][Y] 

- inertia_matrix[Z][Y] * inertia_matrix[X][X])/d; 
inv_inertia_matrix[X][Z] = (inertia_matrix[X][Y] * inertia_matrix[Y][Z] 
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- inertia_matrix[X][Z] * inertia_matrix[Y][Y])/d; 
inv_inertia_matrix[Y][Z] = (inertia_matrix[X][Z] * inertia_matnx[Y][X] 

- inertia_matrix[X][X] * inertia_matrix[Y][Z])/d; 
inv_inertia_malrix[Z][Z] = (inertia_matrix[X][X] * inertia_matrix[Y][Y] 

- inerlia_matrix[X][Y] * inertia_matrix[Y][X])/d; 

} 



/* 

* compute torque value for rotation 
*/ 

calc_torque(vehobj) 
struct vehicle *vehobj; 

{ 

if(vehobj->lforce == vehobj->rforce) 
vehobj->net_torque = 0.0; 

else 

{ 

if(vehobj->scalvel == 0.0) 

{ 

vehobj->net_torque = vehobj->torque * 0.6; 

) 

else 

{ 

vehobj->net_torque = vehobj->torque * 0.8; 

} 

} 

if(vehobj->type == WHEELED && vehobj->scalvel == 0.0) 
vehobj->net_torque = 0.0; 



r 

* compute the total linear forces to the vehicle 
*/ 

sum_linear_forces(vehobj, deltatime) 
struct vehicle *vehobj; 
float deltatime; 

{ 

struct vehicle tvehpos; 
tvehpos = *vehobj; 
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if(Stop_Engine == FALSE) 

{ 

calc_drag_force(&tvehpos); /* compute friction and air resistance */ 
calc_gforce(&tvehpos); /* compute gravity force */ 
calc_engine_force(&tvehpos); /* compute engine force */ 



/* compute the total forces 

each forces are scalar values, so those values can be summed linearly */ 
tvehpos. force = tvehpos.drag_force-Kvehpos.surge_force+tvehpos.g_force; 

if(tvehpos.force > MAXFORCE) tvehpos. force = MAXFORCE; 
if(tvehpos.force < MINFORCE) tvehpos. force = MINFORCE; 

*vehobj = tvehpos; 

} 

/* 

/* compute the total angular forces to the vehicle 
*/ 

sum_angular_forces(vehobj) 
struct vehicle *vehobj; 

( 

float abstorque; 

/* compute the angular force to the vehicle by the right hand rule */ 
vehobj->torque = vehobj->lforce - vehobj->rforce; 

/* absolute value of torque */ 
abstorque = fabs(vehobj->torque); 

/* for the smooth motion for the wheeled vehicle */ 
if(vehobj->type == WHEELED && abstorque <= 100.0) 
vehobj->torque = 0.0; 

if(vehobj->torque >= MAXPOWER) vehof)j->torque = MAXPOWER; 
if(vehobj->torque <= MINPOWER) vehobj->torque = MINPOWER; 

) 

/* 

* compute the linear acceleration of the vehicle 
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*/ 

calc_acceleration(vehobj) 
struct vehicle *vehobj; 

{ 

/* compute acceleration by the Newton’s second law F = ma */ 
vehobj->scalacc = vehobj->force / vehobj->mass; 

/* divide the accleration to x axis and z axis */ 
vehobj->acc[X] = vehobj->scalacc * fcos(vehobj->direction); 
vehobj->acc[Z] = vehobj->scalacc * fsin(vehobj->direction); 

} 

/* 

* compute the linear velocity of the vehicle 
*/ 

calc_velocity(vehobj, delta time) 
struct vehicle *vehobj; 
float deltatime; 

{ 

/* compute the speed from the acceleration in m/sec unit */ 
vehobj->scalvel += deltatime * vehobj->scalacc; 

/* to avoid the numerical instability near 0 speed */ 
if(vehobj->scalvel <= 0.3 && vehobj->scalvel >=-0.3) 
vehobj->scalvel = 0.0; 

/* limit the max speed between minimum and the maximum vehicle speed */ 
if(vehobj->scalvel <= MINSPEED) vehobj->scalvel = MINSPEED; 
if(vehobj->scalvel >= MAXSPEED) vehobj->scalvel = MAXSPEED; 

vehobj->vel[X] = vehobj->scalvel * fcos(vehobj->direction); 
vehobj->vel[Z] = vehobj->scalvel * fsin(vehobj>>direction); 

} 

/* 

* compute angular acceleration and angular velocity by Euler method w = w * dt 
*/ 

calc_direction(vehobj,inertia,deltatime) 
struct vehicle *vehobj; 
float inertia[6]; 
float deltatime; 
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float rotation; /* rotation value by the torque */ 

float dt_rot; /* rotation times deltatime */ 

float ext_rot; /* another rotation value few wheeled vehicle */ 

float tangle; /* tangent angle for wheeled vehicle */ 

if(vehobj->type == TRACKED) 

{ 

/* omega = torque / inertia tensor */ 
rotation = (vehobj->net_torque / inertia[Y]); 
dt_rot = rotation * deltatime; 



if(dt_rot < 0.0) 

dt_rot = dt_rot + DPI; 
else if(dt_rot > DPI) 

dt_rot = dt_rot - DPI; 



) 



vehobj->direction += dt_rot; 
if(vehobj->direction < 0) 

vehobj->direction += DPI; 
else if (vehobj->direction > DPI) 
vehobj->direction -= DPI; 



else /* Wheeled vehicle */ 

{ 

/* compute the difference between the wheel angle and direction */ 
tangle = ftan(vehobj->wheel_angle); 
rotation = fatan(0.5 * tangle); 

/* if there are difference between the left and right forces 
it should be added to the vehicle direction value */ 
if(vehobj->lforce != vehobj->rforce) 

{ 

ext_rot = (vehobj->net_torque / inertia[Y]); 
rotation += ext_rot; 

} 



/* wheeled vehicle cannot rotate without translation */ 
if(vehobj->scalvel != 0.0) 

{ 
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dt_rot = rotation * deltatime; 



if(dt_rot < 0.0) 

dt_rot = dt_rot + DPI; 
else if(dt_rot > DPI) 

dt_rot = dt_rot - DPI; 

vehobj->direction += dt_rot; 

if(vehobj->direction < 0) 

vehobj->direction += DPI; 
else if (vehobj->direction > DPI) 
vehobj->direction -= DPI; 

} 

} 



/* 

* compute the wheel angle to direct to the desired direction 
*/ 

calc_inverse_direction(vehobj,des_direction, delta time) 
struct vehicle *vehobj: 
float *des_direction; 
float deltatime; 

{ 

float rad_direction; 
float del_angle; 

rad_direction = *des_direction; 
if(vehobj->type == WHEELED) 

{ 

if(vehobj->direction != rad_direction) 

{ 

del_angle = fatan(2.0 * ftan(rad_direction)); 
vehobj->wheel_angle = deltatime * del_angle; 

if(vehobj->wheel_angle >= 45.0) 

vehobj->wheel_angle = 45.0; 
if(vehobj->wheel_angle <= *45.0) 

vehobj->wheel_angle = -45.0; 
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else 

{ 

rad_direction = 0.0; 



else /* tracked vehicle */ 

( 

if(vehobj->direction != rad_direction) 

{ 

if(rad_direction > 0.0) 

( 

vehobj->lforce += (rad_direction * 10.0) * (vehobj->mass/l 000.0); 
vehobj->rforce -= (rad_direction * 10.0) * (vehobj->mass /1000.0); 

} 

else /* desired direction is less than 0 */ 

{ 

vehobj->lforce -= (rad_direction * 10.0) * (vehobj->mass /1 000.0); 
vehobj->rforce += (rad_direction * 10.0) * (vehobj->mass /1000.0); 



else 

} 



rad_direction = 0.0; 



*des_direciion = rad_direction; 

} 

/* 

* integrate all forces 
*/ 

integration(vehobj, cruise, behavior ,deltatime) 

struct vehicle *vehobj; 

int *cruise; 

int ^behavior; 

float deltatime; 

{ 

struct vehicle tvehpos; 
float inertia_body[6]; 
float path_direction; 
float inertia_matrix[3][3]; 
int cruise_flag; 
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tvehpos = *vehobj; 

path_direction = tvehpos.direction; /* current vehicle direction */ 
cruise_flag = * cruise; 



if(!*behavior) 

{ 

if(SIDE_BREAK == FALSE) 

( 

/* compute inertia tensor */ 
calc_inertia(&tvehpos,inertia_body,inertia_matrix); 
angle_momentum(&tvehpos); /* compute roll and pitch */ 

if(!cruise_flag) 

{ 

/* compute all the internal and external forces */ 
sum_linear_forces(&tvehpos, deltatime); 
sum_angular_forces(&tvehpos); 

/* compute linear and angular velocities */ 
calc_acceleration(&tvehpos); 
calc_veIocity(&tvehpos,ddtatime); 
calc_torque(& tvehpos) ; 

calc_direction(&tvehpos,inertia_body,ddtatime); 

} 

else /* cruise on */ 

{ 

sumJinear_forces(&tvehpos,ddtatime); 

sum_angular_forces(&tvehpos); 
cru i se_on (& tvehpos) ; 

tvehpos. seal vel = tvehpos.cruise_speed; 

tvehpos.vel[X] = tvehpos.scalvel * fcos(tvehpos.direction); 

tvehpos.velfZ] = tvehpos.scalvel * fsin(tvehpos.direction); 

calc_torque(& tvehpos) ; 

calc_direction(&tvehpos4nertia_body, deltatime); 



else /* when the side break is locked */ 
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tvehpos.rforce = 0.0; 
tvehpos.lforce = 0.0; 
tvehpos.g_force = 0.0; 
tvehpos.dragjorce = 0.0; 
tvehpos.scalvel = 0.0; 



} 

else /* behavioral function */ 

{ 

/* compute inertia tensor */ 
calcJnertia(&tvehpos,inertiaJx>dy,inertia_matrix); 
angle_momentum(&tvehpos); /* compute roll and pitch */ 

/* compute all the internal and external forces */ 
sum_linear_forces(&tvehpos, deltatime); 
sum_angular_forces(&tvehpos); 
cruise_on(&tvehpos); 

tvehpos.scalvel = tvehpos.cruise_speed; 
tvehpos.vel[X] = tvehpos.scalvel * fcos(tvehpos.direction); 
tvehpos.vel[Z] = tvehpos.scalvel * fsin(tvehpos.direction); 

behavior_motion(&tvehpos,&path_direction); 
calc_inverse_direction(&tvehpos,&path_direction, deltatime); 
calc_torque(&tvehpos); 

calc_direction(&tvehpos,inertia_body4eltatime); 

/* inverse dynamics for the vehicle direction */ 

} 

/* compute miscelaneous factors of the vehicle */ 

calc_gas(&tvehpos,deltatime); 

calc_rpm(&tvehpos); 

*vehobj = tvehpos; 



r 

* compute the new x and z position by the linear and angular velocity 
*/ 

new_position(vehobj, cruise, behavior , deltatime) 
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struct vehicle *vehobj; 
int *cruise; 
int *behavior; 
float deltatime; 

{ 

struct vehicle tvehpos; 
int behavior_flag; 
int cruise_flag; 

tvehpos = *vehobj; 
behavior_flag = * behavior; 
cruise_flag = *cruise; 

/* compute acceleration and velocity */ 
integration(&tvehpos,&cruise_flag,&behavior_flag, deltatime); 

/* Euler Integration, Pn = Pn-1 + (Vt * dT) + (ACC * dT * dT / 2) */ 

tvehpos .pos[X] += (tvehpos.vel[X] * deltatime) + (tvehpos .acc[X] * deltatime * deltatime/ 2.0); 
tvehpos .pos[Z] += (tvehpos. vel[Z] * deltatime) + (tvehpos. acc[Z] * deltatime * deltatime / 2.0); 
tvehpos .pos[Y] = gnd_level(tvehpos.pos[X],tvehpos.pos[Z]); 

*vehobj = tvehpos; 

} 
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APPENDIX C 



Source Code For Behavioral Motion 



/* 

* determine the search iri and find any objects in there 

*/ 

#include “vehsim.h” 

#include “vehcle.h” 

#include “extems.h” 

/* Need a variable to hold the radius of the vehicles’ turning that appropriate value to avoid the object */ 
float RADIUS = 5.0; 

int distbetween2(); /* This function finds the distance between 2 points */ 
int seq_search(); /* search obstacle table array */ 

/* 

* detect non-moving obstacles 
*/ 

detect_nonmoving_objects(vehobj) 

struct vehicle *vehobj; /* actual object being checked for avoidance */ 

( 

float radius_of_vehicle_and_object; 
int xgrid, zgrid; /* gridsquare indices */ 

xgrid = (int)(vehobj->pos[X]/GRIDSIZE); 
zgrid = (int)(vehobj->pos[Z]/GRIDSIZE); 

} 



/* 

*this function constructs the search triangle given the at look at point 
*/ 

detect_bounds(vehobj,vehdir,searchtri t advance) 
struct vehicle *vehobj; 
float *vehdir; 
float searchtri[4][3]; 
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float advance; 



{ 

float segdir; 
float langle, rangle; 

segdir = *vehdir; 

/* determine the angles of the view bounds */ 
rangle = segdir - HFOS; 
langle = segdir + HFOS; 

if(rangle < 0.0) rangle = DPI + rangle; 
if(langle > DPI) langle = langle * DPI; 

/* compute the center vertex of the triagnle */ 
searchtri[2][X] = vehobj->pos[X]; 
searchtri[2][Z] = vehobj->pos[Z]; 

/* compute the left vertex of the triagnle */ 

searchtri[0][X] = searchtri[2][X] + fcos(langle) * (SEARCHOFFSET+advance); 
searchtrifO] [ZJ = search tri [2] [Z] + fsin(langle) * (SEARCHOFFSET+advance); 

/* compute the right vertex of the triagnle */ 

searchtri[l][X] = searchtri[2][X] + fcos(rangle) * (SEARCHOFFSET+advance); 
searchtri[l][Z] = searchtri[2][Z] + fsin(rangle) * (SEARCHOFFSET+advance); 

/* compute the center vertex between the left and right vertices */ 
searchtri[3][X] = (searchtri[0][X] + searchtri[l][X])/2.0; 
searchtri[3][Z] = (searchtri[0][Z] + searchtrif 1 ] [Z]) /2.0; 

} 

/* 

* serach obstacle array for avoidance 
*/ 

search_object(searchtri,find_objectJrvalue,vehicle_dir) 

float searchtri[4][3]; 

int *find_object; 

int *lrvalue; 

float vehicle_dir; 

{ 
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float leftminx/ightminxjeftminz/ightrninz; 
float leftmaxx/ightmaxxjeftmaxz/ightmaxz; 
float lobjx, lobjz, robjx, robjz; 
int left_search,right_search,overlapped; 
int intdir; 

float inverse_dir, tempx, tempz; 
int Idistance^distance; 

/* reinitialize the flags */ 

*find_object = FALSE; 
left_search = FALSE; 
right_search = FALSE; 

boundaryC&searchtri^ltXl.&searchtritOlM.&searchtritSltXi^&leftminx^&leftmaxx); 

boundary(&searchtri[2][X] > &searchtri[l]pC] t &searchtri[3][X],&righuninx,&rightmaxx); 

boundary(&searchtri[2][Z],&searchtri[0][Z],&searchtri[3][Z],&leftminz,&leftmaxz); 

boundary(&searchtrif2]rZ] > &searchtri[l][Z],&searchtri[3][Z],&rightminz,&righunaxz); 

left_search = seq_search(&leftminx > &leftmaxx,&leftminz > &leftmaxz,&lobjx > &lobjz); 
right_search = seq_search(&rightminx > &rightmaxx,&rightminz > &rightmaxz > &robjx»&robjz); 

/* check if the object is in between the left tri and right tri */ 
if((lobjx == robjx) && (lobjz = robjz)) { 
overlapped = TRUE; 



else { 

overlapped = FALSE; 

} 



if(vehicle_dir < 0.0) 

vehicle_dir += DPI; 

if(vehicle_dir -- 0.0 II vehicle_dir == 360.0) 
intdir = 1; 

else if(vehicle_dir > 0.0 && vehicle_dir < QTR_PI) 
intdir = 2; 

else if(vehicle_dir == QTR_PI) 
intdir = 3; 

else if(vehicle_dir > QTR_PI && vehicle_dir < HALFPI) 
intdir = 4; 

else if(vehicle_dir == HALFPI) 
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intdir = 5; 

else if(vehicle_dir > HALFPI && vehicle_dir < THREE_QTR_PI) 
intdir = 6; 

else if(vehicle_dir == THREE_QTR_PI) 
intdir = 7; 

else if(vehicle_dir > THREE_QTR_PI && vehicle_dir < DPI) 
intdir = 8; 



if(overlapped) 

{ 

switch(intdir) 

( 

case 1: /* direction = 0 or 2pi */ 

if(searchtri[2][X] >= lobjx) 

*lrvalue = LEFT; 

else 

*lrvalue = RIGHT; 

break; 

case 2: /* 0 < direction < 0.5pi */ 

inverse_dir = QTR_PI - vehicle_dir; 
tempx = fabs(lobjx - searchtri[2][X]); 
tempz = ftan(inverse_dir) * tempx; 
tempz = searchtri[2][Z] - tempz; 
if(lobjz >= tempz) 

*lrvalue = RIGHT; 

else 

*lrvalue = LEFT; 

break; 

case 3: /* direction = 0.5pi*/ 

if(searchtri[2][Z] >= lobjz) 

*lrvalue = LEFT; 

else 

*lrvalue = RIGHT; 

break; 

case 4; /* 0.5pi < direction < pi */ 
inverse_dir = vehicle_dir; 
tempz = fabs(lobjz - searchtri[2][Z]); 
tempx = fabs(ftan(inverse__dir) * tempz); 
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tempx = searchtri[2][X] + tempx; 
if(lobjx >= tempx) 

*irvalue = LEFT; 
else 

*irvalue = RIGHT; 

break; 



case 5: /* direction = pi */ 

if(searchtri[2][X] <= lobjx) 

*lrvalue = LEFT; 
else 

*lrvalue = RIGHT; 

break; 

case 6: /* pi < direction < 1.5pi */ 

inverse_dir = vehicle_dir - PI; 
tempz = fabs(Iobjz - search tri [2] [Z]); 
tempx = fabs(ftan(inverse_dir) * tempz); 
tempx = searchtri[2][X] - tempx; 
if(lobjx >= tempx) 

*lrvalue = LEFT; 
else 

*lrvalue = RIGHT; 



break; 

case 7: /* direction = 1.5pi*/ 

if(searchtri[2][Z] <= lobjz) 

*lrvalue = LEFT; 
else 

*lrvalue = RIGHT; 

break; 

case 8: /* 1.5pi < direction <= 2pi */ 

inverse_dir = vehicle_dir - THREE_QTR_PI; 
tempx = fabs(lobjx - searchtri[2][X]); 
tempz = fabs(ftan(inverse_dir) * tempx); 
tempz = searchtri[2][Z] - tempz; 
if(lobjz >= tempz) 

*lrvalue = LEFT; 
else 



60 



♦lrvalue = RIGHT; 



} 



break; 



*find_object - TRUE; 

} 



else /* the object is in left tri or right tri */ 

{ 

if(left_search && right_search) 

{ 

ldistance = dist_in_2d(lobjx, lobjz, searchtri[2][X], searchtri[2][Z]); 
rdistance = dist_in_2d(robjx, robjz, searchtri[2][X], searchtri[2][Zj); 



if(ldistance >= rdistance) 
*lrvalue - LEFT; 

else 

*lrvalue = RIGHT; 



} 

else /* among right, left or none object */ 

{ 

if(left_search) 



*find_object = TRUE; 
*lrvalue = LEFT; 

} 

else 



if(right_search) 

{ 

*find_object = TRUE; 
*lrvalue = RIGHT; 



else /* !left_search && !right_search */ 

{ 

*find_object = FALSE; 



} 



} 
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} /* the outer else */ 



) 

seq_search(minx,maxx,minz,maxz,objx,objz) 
float *minx, *maxx, *minz, *maxz, *objx, *objz; 

{ 

int ix; 

float tempobjx, tempobjz; 

for(ix=0;ix<=100;ix++) 

( 

tempobjx = avoidlist[ix].center[X]; 
tempobjz = avoidlist[ix].center[Z]; 

if(tempobjx >= *minx && tempobjx <= *maxx && 

tempobjz >= *minz && tempobjz <= *maxz) 

( 

*objx = tempobjx; 

*objz = tempobjz; 
retum(TRUE); 



retum(FALSE); 



/* 

* determine the minimum and maximum number among the three floating point numbers 

*/ 

boundary(x 1 ,x2,x3,minno,maxno) 
float *xl,*x2,*x3; 
float *minno,*maxno; 

{ 

float ix ,jx,kx; 

ix = *xl; 
jx = *x2; 
kx = *x3; 

*minno = ix; 

*maxno = ix; 
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if (jx < *minno) *minno = jx; 
if (jx > * max no) * max no = jx; 
if (kx < *minno) *minno = kx; 
if (kx > * max no) *maxno = kx; 



/*this function computes the 3D distance between two points, it returns an interger*/ 
dist_in_2d(subjectx,subjectz,objectx, object z) 
float subjectx,subjectz,objectx,objectz; 

{ 

float distx, distz; 
int dist; 

distx = subjectx - objectx; 

distx = distx * distx; 

distz = subjectz - objectz; 

distz = distz * distz; 

dist = (int)fsqrt((distx+distz)); 

retum(dist); 
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